#include "FrenetOptimalTrajectory.h"


FrenetOptimalTrajectory::FrenetOptimalTrajectory(/* args */) { }

FrenetOptimalTrajectory::~FrenetOptimalTrajectory() {}

void FrenetOptimalTrajectory::calc_frenet_paths(FrenetInitialConditions* fot_ic, FrenetHyperparameters* fot_hp,
    std::priority_queue<std::unique_ptr<FrenetPath>, std::vector<std::unique_ptr<FrenetPath>>, CostComparator_for_Frenet>& frenet_path, 
    const std::vector<std::pair<double, double>>& global_points, const std::vector<double>& acc_s, 
    const int& index, const std::unique_ptr<CubicSpline2D>& Cub_s)
{
    double t, ti, tv;   // t从0开始，间隔DT,加到ti，tv：纵向速度规划
    double lateral_deviation, lateral_velocity, lateral_acceleration, lateral_jerk;
    double longitudinal_acceleration, longitudinal_jerk;
    double di;    //横向宽度采样
    //%调用类，FrenetPath里面定义了一系列参数
    //这里分两条写了。tfp要复制fp有的参数值，最后使用tfp就可以
    //生成每个偏移目标的路径，即d-t坐标系下的路径轨迹和s-t下的速度轨迹
    if(abs(fot_hp->Min_road_width) >= 1.0){
        di = fot_hp->Min_road_width;
    }else{
        di = fot_hp->middle_road_;
    }
    while (di <= fot_hp->Max_road_width + fot_hp->D_road_w)
    {
        ti = fot_hp->MinT;
        while (ti <= fot_hp->MaxT + fot_hp->Dt)
        {
            //初始化用于累加的参数
            lateral_deviation = 0;
            lateral_velocity = 0;
            lateral_acceleration = 0;
            lateral_jerk = 0;
            std::unique_ptr<FrenetPath> fp = std::make_unique<FrenetPath>();
            // d0, dd0, ddd0为初始配置， di, ddT, dddT为目标配置
            //计算出关于目标配置di，Ti的横向多项式，5次多项式
            QuinticPolynomial lat_qp = QuinticPolynomial(fot_ic->d0, fot_ic->dd0, fot_ic->ddd0, di, 0, 0, ti);
            // 构建frenet路径，t从0开始，间隔DT,加到ti
            t = 0.0;
            while (t <= ti + fot_hp->Dt) //其实就是0-ti
            {
                //存储d/t坐标系的值
                fp->t.push_back(t);                                                                                //时间戳存储，这样写是生成数组
                fp->d.push_back(lat_qp.quint_point(t));                                    //存储横向偏移
                fp->d_d.push_back(lat_qp.quint_first_derivative(t));           //取一阶导数值
                fp->d_dd.push_back(lat_qp.quint_first_derivative(t));        //取二阶导数值
                fp->d_ddd.push_back(lat_qp.quint_third_derivative(t));   //取三阶导数值
                //累加，为了求代价函数
                lateral_deviation += fabs(lat_qp.quint_point(t));                                    //横向偏差累加
                lateral_velocity += fabs(lat_qp.quint_first_derivative(t));                   //横向速度累加
                lateral_acceleration += fabs(lat_qp.quint_first_derivative(t));          //横向加速度累加
                lateral_jerk += fabs(lat_qp.quint_third_derivative(t));                         //横向加加速度累加
                t += fot_hp->Dt;                                                                                                      //加Dt,循环
            }
            tv = fot_ic->target_speed - fot_hp->D_t_s * fot_hp->N_s_sample;
            while (tv <= fot_ic->target_speed + fot_hp->D_t_s * fot_hp->N_s_sample + fot_hp->D_t_s)
            {
                longitudinal_acceleration = 0;
                longitudinal_jerk = 0;
                // copy frenet path
                std::unique_ptr<FrenetPath> tfp = std::make_unique<FrenetPath>();
                tfp->t.assign(fp->t.begin(), fp->t.end());
                tfp->d.assign(fp->d.begin(), fp->d.end());
                tfp->d_d.assign(fp->d_d.begin(), fp->d_d.end());
                tfp->d_dd.assign(fp->d_dd.begin(), fp->d_dd.end());
                tfp->d_ddd.assign(fp->d_ddd.begin(), fp->d_ddd.end());
                //四次多项式
                QuarticPolynomial lon_qp = QuarticPolynomial(fot_ic->s0, fot_ic->ds0, 0.0, tv, 0.0, ti);
                // 纵向规划
                for (double tp : tfp->t) //其实就是0-ti,tfp->t为数组，每个值间隔Dt
                {
                    //存储s/t坐标系的值
                    tfp->s.push_back(lon_qp.quart_point(tp));                //取值
                    tfp->s_d.push_back(lon_qp.quart_first_derivative(tp));   //取一阶导数值
                    tfp->s_dd.push_back(lon_qp.quart_second_derivative(tp)); //取二阶导数值
                    tfp->s_ddd.push_back(lon_qp.quart_third_derivative(tp)); //取三阶导数值
                    //累加，为了求代价函数
                    longitudinal_acceleration += fabs(lon_qp.quart_second_derivative(tp));
                    longitudinal_jerk += fabs(lon_qp.quart_third_derivative(tp));
                }
                // 失败或路径无效时删除
                //每次生成一条轨迹,就进行检测,检测不过之间删除并跳到下一个循环
                bool success = to_global_path(tfp, global_points, acc_s, index, Cub_s);
                if (!success || !is_valid_path(tfp))
                {
                    tv += fot_hp->D_t_s;
                    continue;
                }

                //横向的代价
                tfp->c_lateral_deviation = lateral_deviation;
                tfp->c_lateral_velocity = lateral_velocity;
                tfp->c_lateral_acceleration = lateral_acceleration;
                tfp->c_lateral_jerk = lateral_jerk;
                tfp->Jd = fot_hp->kj * tfp->c_lateral_jerk + fot_hp->kd * tfp->c_lateral_deviation +
                                    fot_hp->kv * tfp->c_lateral_velocity + fot_hp->kt * ti;
                /* tfp->wd = fot_hp->wlat_d *  tfp->c_lateral_deviation + fot_hp->wlat_v * tfp->c_lateral_velocity + 
                                            fot_hp->wlat_a * tfp->c_lateral_acceleration + fot_hp->wlat_j * tfp->c_lateral_jerk + fot_hp->wlon_t* ti; */
                //纵向的代价
                tfp->c_longitudinal_acceleration = longitudinal_acceleration;
                tfp->c_longitudinal_jerk = longitudinal_jerk;
                tfp->c_end_speed_deviation = pow((fot_ic->target_speed - tfp->s_d.back()), 2); //速度差
                tfp->c_time_taken = ti;
                /* tfp->ws = fot_hp->wlon_a * tfp->c_longitudinal_acceleration + fot_hp->wlon_j * tfp->c_longitudinal_jerk + 
                                                    fot_hp->wlon_v * tfp->c_end_speed_deviation + fot_hp->wlon_t * tfp->c_time_taken;
                tfp->W = fot_hp->wlat_score * tfp->wd + fot_hp->wlon_score * tfp->ws; */
                tfp->Js = fot_hp->kj * tfp->c_longitudinal_jerk + fot_hp->kt * tfp->c_time_taken +
                                                fot_hp->kd * tfp->c_end_speed_deviation + fot_hp->ka * tfp->c_longitudinal_acceleration;
                tfp->J = fot_hp->klat * tfp->Jd + fot_hp->klon * tfp->Js;
                frenet_path.emplace(std::move(tfp));

                tv += fot_hp->D_t_s;
            }
            ti += fot_hp->Dt;
        }
        di += fot_hp->D_road_w;
    }
    
}

bool FrenetOptimalTrajectory::to_global_path(std::unique_ptr<FrenetPath>& tfp, 
                                                                const std::vector<std::pair<double, double>>& global_points,
                                                                const std::vector<double>& acc_s, const int& index,
                                                                const std::unique_ptr<CubicSpline2D>& Cub_s)
{
    double dx, dy;
    if(global_points.size() <= 1 || !tfp) 
    {
        return false;
    }

    for(size_t i = 0; i < tfp->s.size(); i++)
    {
        SplineResult cub_result = Cub_s->calc_total(tfp->s[i]);
        double ix_ = cub_result.x;
        double iy_ = cub_result.y;
        if (isnan(ix_) || isnan(iy_)) //如果ix_,iy_不存在
            break;
        double iyaw_ = cub_result.yaw;
        double di = tfp->d[i];
        double fx = ix_ + di * cos(iyaw_ + M_PI_2);
        double fy = iy_ + di * sin(iyaw_ + M_PI_2);
        tfp->x.push_back(fx);
        tfp->y.push_back(fy);
        tfp->v.push_back(tfp->s_d[i]);
        tfp->a.push_back(tfp->s_dd[i]);
        tfp->kappas.push_back(cub_result.kappa);
    }

    if(tfp->x.size() <= 1)
    {
        return false;
    }

        // 计算 theta and dL (running length)
        for (size_t i = 0; i <  tfp->x.size() - 1; i++)
        {
            dx = tfp->x[i + 1] - tfp->x[i];
            dy = tfp->y[i + 1] - tfp->y[i];
            tfp->theta.push_back(atan2(dy, dx));   //航向角
            tfp->dL.push_back(hypot(dx, dy));    //垂直里程
        }
        tfp->theta.push_back(tfp->theta.back()); //.back()返回vetor最末尾的值
        tfp->dL.push_back(tfp->dL.back());
    return true;
}

bool FrenetOptimalTrajectory::is_valid_path(std::unique_ptr<FrenetPath>& tfp)
{
    // max speed check
    if (any_of(tfp->s_d.begin(), tfp->s_d.end(), [this](int i)
    { return fabs(i) > 3; }))
    {
        return false;
    }
    // max accel check
    else if (any_of(tfp->s_dd.begin(), tfp->s_dd.end(), [this](int i)
                { return fabs(i) > 2; }))
    {
        return false;
    }
    // max curvature check
    else if (any_of(tfp->kappas.begin(), tfp->kappas.end(), [this](int i)
                { return fabs(i) > 2; }))
    {
        return false;
    }

    return true;
}

DiscretizedTrajectory FrenetOptimalTrajectory::FrenetPath_to_TrajectoryPoint(std::unique_ptr<FrenetPath>& best_path)
{
    DiscretizedTrajectory Optim_trajectory;
  if (best_path == nullptr){
    return {};
  }
  // double kappas = best_path->kappa;
  // LOG(WARNING) << kappas;
  for (int i = 0; i < best_path->x.size(); ++i)
  {
    TrajectoryPoint trajectory_point;
    //赋值frenet坐标系下的
    trajectory_point.set_relative_time(best_path->t[i]);
    trajectory_point.set_s(best_path->s[i]);
    trajectory_point.s_d = best_path->s_d[i];
    trajectory_point.s_dd = best_path->s_dd[i];
    trajectory_point.d = best_path->d[i];
    trajectory_point.d_d = best_path->d_d[i];
    trajectory_point.d_dd = best_path->d_dd[i];
    //赋值世界坐标系下的
    trajectory_point.set_x(best_path->x[i]);
    trajectory_point.set_y(best_path->y[i]);
    trajectory_point.set_theta(best_path->theta[i]);
    // trajectory_point.set_kappa(0);
    trajectory_point.set_kappa(best_path->kappas[i]);
    trajectory_point.set_v(best_path->v[i]);
    trajectory_point.set_a(best_path->a[i]);

    Optim_trajectory.emplace_back(trajectory_point);
  }

  return Optim_trajectory;
}
